package edu.wpi.first.wpilibj.swarm;

import edu.wpi.first.wpilibj.Relay;
import edu.wpi.first.wpilibj.AnalogChannel;
import edu.wpi.first.wpilibj.Solenoid;
import edu.wpi.first.wpilibj.Victor;
import edu.wpi.first.wpilibj.Utility;

public class Kicker implements Runnable {
	volatile double tgtpos;
	double          brush_command;
	Relay           winch;
	AnalogChannel   pot;
	Solenoid        release;
	Solenoid        engage;
	boolean         kicking;
	Victor          brush;
	private boolean kickreset;
	private boolean enabled;
	
	static final double forw_voltage         = 3.4;
	static final double back_voltage         = 4.7;
	
	static final double hysteresis           = .03;
	
	Kicker() {
		winch   = new Relay(1);
		pot     = new AnalogChannel(5);
		pot.setAverageBits(9);
		engage  = new Solenoid(1);
		release = new Solenoid(2);
		kicking = false;
		kickreset = true;
		brush   = new Victor(6);
		tgtpos  = 0.0;
		enabled = false;
		init();
	}
	
	private void init() {
		new Thread(this).start();
	}
	
	void disengageWinch() {
		engage.set(false);
		release.set(true);
	}
	
	void engageWinch() {
		release.set(false);
		engage.set(true);
	}
	
	double getPos() {
		return (pot.getAverageVoltage() - forw_voltage) / (back_voltage-forw_voltage);
	}
	
	void setKicking(boolean kick) {
		if (!kick) {
			kicking = false;
			kickreset = true;
		} else if (kick && kickreset) {
			kickreset = false;
			kicking = true;
		}
	}
	
	private void brushApply() {
		if (!kicking) brush.set(brush_command);
	}
	
	void brushOn() {
		brush_command = -0.8;
		brushApply();
	}
	
	void brushOff() {
		brush_command = 0.0;
		brushApply();
	}
	
	void brushRev() {
		brush_command = -0.4;
		brushApply();
	}
	
	void setPos(double target) {
		tgtpos = target;
	}
	
	boolean isKicking() {
		return kicking;
	}

	public void run() {
		long time = Utility.getFPGATime();
		engageWinch();
		double pos;
		while(true) {
			if (enabled) {
				if (kicking) {
					if (getPos() >= tgtpos) executeKick();
					else                    winch.set(Relay.Value.kReverse);
				} else {
					pos = getPos();
					if (Math.abs(pos) < hysteresis) {
						winch.set(Relay.Value.kOff);
					} else if (pos > hysteresis) {
						winch.set(Relay.Value.kForward);
					} else if (pos < -hysteresis) {
						winch.set(Relay.Value.kReverse);
					}
				}
			} else {
				winch.set(Relay.Value.kOff);
			}
			
			time += 20000;
			if (time > Utility.getFPGATime() + 1000) {
				try {
					Thread.sleep((time - Utility.getFPGATime())/1000);
				} catch (InterruptedException ex) {}
			}
		}
	}
	
	void wait(double time) {
		try {
			Thread.sleep((long) (time*1000.0));
		} catch (InterruptedException ex) {}
	}

	private void executeKick() {
		brush.set(0.0);
		winch.set(Relay.Value.kOff);
		disengageWinch();
		kicking = false;
		wait(1.0);
		
		brush.set(brush_command);
		engageWinch();
	}
	
	void setEnabled(boolean enable) {
		enabled = enable;
	}
}